#include <limits>
#include <list>
#include <cassert>

#include "mapObject.h"
#include "objectstorage.h"
#include "geometry.h"
#include "rtree.h"

class RTreeNode {
    friend class RTree;

    RTreeNode(RTree *tr, uint par, uint fig, uint hnd) : tree(tr), handle(hnd), 
                                                         parent(par), figure(fig), 
                                                         first(0), next(0), count(0)
    {
        tree->nodes.push_back(this);

        if (figure) {
            boundRect = tr->primitive(figure)->boundingRect();
        }
        if (parent) {
            tree->fromHandle(parent)->appendSon(handle);
        }
    }

    ~RTreeNode() {
        RTreeNode *child = tree->fromHandle(first);
        while (child) {
            RTreeNode *next = tree->fromHandle(child->next);
            delete child;
            child = next;
        }
    }

    void appendSon(uint node)
    {
        uint prev = first;
        first = node;
        RTreeNode *son = tree->fromHandle(first);
        son->next = prev;
        son->parent = this->handle;
        count++;
    }

    void removeChilds() {
        first = 0;
        count = 0;
    }

    void query ( const QRectF &rect, std::vector<PrimitiveBase *> &res ) 
    {
        QRectF common = rect.intersect(boundRect);
        if (common.isEmpty()) {
            return;
        }

        for (RTreeNode *son = tree->fromHandle(first); son; son = tree->fromHandle(son->next)) {
            son->query(rect, res);
        }

        if (first == 0) {
            res.push_back(tree->primitive(figure));
        }
    }

    bool isLeaf() {
        return first == 0;
    }

    int soncount() {
        return count;
    }

    uint next;
    uint first;
    uint handle;
    uint parent;

    RTree *tree;

    QRectF boundRect;
    uint figure;
    int count;
};

RTree::RTree(ObjectStorage *st, int least_, int most_) : storage(st), least(least_), most(most_) {
    curHandle = 1;
    RTreeNode *rt = new RTreeNode(this, 0, 0, curHandle++);
    root = rt->handle;
    preloaded = false;
}

RTree::~RTree() {
    RTreeNode *rt = fromHandle(root);
    delete rt;
}

void RTree::query ( const QRectF &rect, std::vector<PrimitiveBase *> & primitives )
{
    if (root)
    {
        fromHandle(root)->query ( rect, primitives );
    }
}

void RTree::verify ( RTreeNode *node )
{    
    if ( node -> isLeaf()) 
    {
        assert ( node -> figure );
        assert ( node -> boundRect == primitive(node -> figure) -> boundingRect ());
    }
    else 
    {
        QRectF common = fromHandle(node -> first) -> boundRect;
        for (RTreeNode *son = fromHandle(node->first); son; son = fromHandle(son->next))
        {
            verify ( son);
            QRectF rect = son -> boundRect;
            common = common.unite(rect); // DANGER! Chagned "merge" to "unite".
            assert ( son -> parent == node->handle );
        }
        assert(node->boundRect == common);
    }
}

void RTree::insert(PrimitiveBase *figure) {
    RTreeNode *node = findSubtree(figure);
    RTreeNode *entity = new RTreeNode(this, node->handle, figure->getID(), curHandle++);
    if (node->soncount() == 1) {
        node->boundRect = entity->boundRect;
    }

    const QRectF figureRect = figure->boundingRect();

    while ( node ) 
    {
        node -> boundRect = node -> boundRect.unite ( figureRect );

        if ( node -> soncount() == most + 1 ) 
        {
            if ( node->handle == root ) 
            {
                RTreeNode * save = fromHandle(root);
                RTreeNode * rt =  new RTreeNode (this, 0, NULL, curHandle++);
                rt -> appendSon(save->handle);
                save -> parent = rt->handle;
                rt -> boundRect = save -> boundRect;
                root = rt->handle;
            }
            split(node);
        }

        node = fromHandle(node->parent);
    }

#ifdef _DEBUG
    //verify(root);
#endif
}

RTreeNode *RTree::findSubtree ( PrimitiveBase *figure ) 
{
    RTreeNode * node = fromHandle(root);
    QRectF rect = figure->boundingRect();

    while ( ! node -> isLeaf ()) 
    {
        RTreeNode * min;
        double found = FLT_MAX;
        double common = FLT_MAX;

        for (RTreeNode *son = fromHandle(node->first); son; son = fromHandle(son->next)) 
        {
            QRectF bounds = son -> boundRect;
            double enlarge = rectArea ( bounds.unite ( rect )) - rectArea ( bounds );
            if ( enlarge < found || 
                ( enlarge == found && common > rectArea ( bounds ) + enlarge )) 
            {
                min = son;
                common = rectArea ( bounds ) + enlarge;
                found = enlarge;
            }
        }
        node = min;
    }

    if (node->handle != root) {
        node = fromHandle(node->parent);
    }
    return node;
}

void RTree::split(RTreeNode * node) {
    const int count = node->soncount();

    // select two entries to be first entries in the groups
    int first, second;
    double found = -1e6;

    std::vector<RTreeNode *> sons;
    for (RTreeNode *son = fromHandle(node->first); son; son = fromHandle(son->next)) {
        sons.push_back(son);
    }

    for (int i = 0; i < count; i++) 
    {
        for (int j = 0; j < count; j++) 
        {
            QRectF f = sons[i]->boundRect;
            QRectF s = sons[j]->boundRect;
            double common = rectArea ( f.unite ( s ));
            double diff = common - rectArea ( f ) - rectArea ( s );
            if ( diff > found ) 
            {
                found = diff;
                first = i;
                second = j;
            }
        }
    }

    std::vector<RTreeNode *> a;
    std::vector<RTreeNode *> b;
    a.push_back(sons[first]);
    b.push_back(sons[second]);

    std::vector<bool> removed(count);
    removed[first] = true;
    removed[second] = true;

    QRectF arect = sons[first]->boundRect;
    QRectF brect = sons[second]->boundRect;


    while (a.size() + b.size() != count) {
        if (a.size() == count - least) {
            for (int i = 0; i < count; i++) {
                if (!removed[i]) {
                    b.push_back(sons[i]);
                    brect = brect.unite ( sons[i] -> boundRect );
                    removed[i] = true;                                       
                }
            }
        }
        else if ( b.size() == count - least ) 
        {
            for (int i = 0; i < count; i++) 
            {
                if (!removed[i] ) 
                {
                    a.push_back ( sons[i] );
                    arect = arect.unite ( sons[i] -> boundRect );
                    removed[i] = true;
                }                    
            }
        }
        else 
        {
            // select next entry to be distributed
            int next;
            double found = -1;
            for (int i = 0; i < count; i++) 
            {
                if ( ! removed[i] )
                {
                    QRectF rect = sons[i] -> boundRect;
                    double d1 = rectArea ( arect.unite ( rect )) - rectArea ( arect );
                    double d2 = rectArea ( brect.unite ( rect )) - rectArea ( brect );
                    double diff = abs ( d2 - d1 );
                    if ( diff > found ) 
                    {
                        found = diff;
                        next = i;
                    }
                }
            }

            QRectF selected = sons[next] -> boundRect;
            double aenl = rectArea ( selected.unite( arect )) - rectArea ( arect );
            double benl = rectArea ( selected.unite( brect )) - rectArea ( arect );

            bool toA = false;
            if ( aenl < benl ) 
            {
                toA = true;
            }
            else if ( aenl == benl ) 
            {
                if ( rectArea ( arect ) < rectArea ( brect )) 
                {
                    toA = true;
                }
                else if ( rectArea ( arect ) == rectArea ( brect )) 
                {
                    toA = a.size() < b.size();
                }
            }

            if ( toA ) 
            {
                a.push_back ( sons[next] );
                arect = arect.unite ( selected );
            }
            else 
            {
                b.push_back ( sons[next] );
                brect = brect.unite ( selected );
            }
            removed[next] = true;
        }
    }

    node -> boundRect = arect;
    node -> removeChilds();
    for (std::vector<RTreeNode *>::iterator it = a.begin(); it != a.end(); it++) {
        node->appendSon((*it)->handle);
    }

    RTreeNode * parent = fromHandle(node -> parent);
    RTreeNode * created = new RTreeNode (this, parent->handle, NULL, curHandle++);
    for (std::vector<RTreeNode *>::iterator it = b.begin(); it != b.end(); it++) {
        created->appendSon((*it)->handle);
    }    
    created -> boundRect = brect;
}

QRectF RTree::boundingRect()
{
    if (root)
    {
        return fromHandle(root)->boundRect;
    }
    else
    {
        return QRectF();
    }
}

RTreeNode * RTree::fromHandle(uint handle)
{
    if (handle)
    {
        return nodes[handle - 1];
    }
    return 0;
}

void RTree::write(FILE *fp)
{
    fwrite(&root, sizeof(int), 1, fp);
    for (uint i = 0; i < nodes.size(); i++) {
        fwrite(nodes[i], sizeof RTreeNode, 1, fp);
    }
}

void RTree::load(FILE *fp)
{
    delete fromHandle(root);
    nodes.clear();

    fread(&root, sizeof(int), 1, fp);
    while (!feof(fp)) {
        RTreeNode *node = new RTreeNode(this, 0, 0, 0);
        fread(node, sizeof RTreeNode, 1, fp);
        node->tree = this;
    }    
    preloaded = true;
}

PrimitiveBase *RTree::primitive(int id)
{
    return storage->getPrimitiveByID(id);
}

bool RTree::isPreloaded()
{
    return preloaded;
}